;;;
;;; 
;;;

(in-package :dreaming-tree)

(defgeneric distance-squared (point obj))
(defgeneric distance (point obj))
(defgeneric which-side (object plane))

;; Lines, rays, segments are parameterized by P + t*D.
;; D is a unit-length vector. The data structures below
;; represent P and D and t is assumed to have the
;; following values depending on type:
;;
;; Line: t = [-infinity,+infinity]
;; Ray: t = [0,+infinity]
;; Segment: t = [-e, e]


(defun plane3 (a b c)
  "Construct a plane defined by three non-colinear points."
  (let ((p0 (m- b a))
	(p1 (m- c a)))
    (make-instance 'plane :point a :plane-normal (cross p0 p1))))


(defun triangle3 (v0 v1 v2)
  (make-instance 'triangle :base v0
		 :extent0 (m- v1 v0)
		 :extent1 (m- v2 v0)))


(defmethod initialize-instance :after ((p plane) &rest initargs)
  (declare (ignore initargs))
  ;; Makes distance calculations a little faster if we ensure the
  ;; normal vector for the plane is a uni vector
  (setf (plane-normal p) (normalize (copy-vector (plane-normal p)))))


(defun print-plane-details (obj stream)
  (format stream "(N:~A P:~A)" (plane-normal obj) (point obj)))

;;;; === Points and spheres to planes ===
(defmethod distance ((p vector) (plane plane))
  (abs (dot-product (plane-normal plane) (m- p (point plane)))))

(defmethod distance ((s sphere) (plane plane))
  (let* ((d (distance (center s) plane))
	 (ds (- d (radius s))))
    (if (< ds +epsilon+)
	0.0
	ds)))

(defmethod which-side ((p vector) (plane plane))
  (let* ((d (dot-product (plane-normal plane) (point plane)))
	 (pos (- (dot-product p (plane-normal plane)) d)))
    (cond ((plusp pos) 1)
	  ((zerop pos) 0)
	  (t -1))))

(defmethod which-side ((s sphere) (plane plane))
  (if (< (distance s plane) +epsilon+)
      0
      (which-side (center s) plane)))

;(defmethod which-side ((box aabb) (plane plane))
;  (if (< (distance ))))
;;;; === Point to a Line ===

(defun %point-line-dsqr (p l)
  (let* ((diff (m- p (point l)))
	 (closest (dot-product (direction l) diff)))
    (setq diff (m- diff (m* closest (direction l))))
    (values (squared-magnitude diff) closest)))

(defmethod distance-squared ((p vector) (l line))
  (%point-line-dsqr p l))

(defmethod distance-squared ((q vector) (r ray))
  (multiple-value-bind (dist closest) (%point-line-dsqr q r)
    (when (< closest 0.0)
      (setq closest 0.0)
	(setq dist (squared-magnitude (m- q (point r)))))
    (values dist closest)))

(defmethod distance-squared ((q vector) (s segment))
  (multiple-value-bind (dist closest) (%point-line-dsqr q s)
    (with-slots (e) s
      (cond ((< closest (- e))
	     (values (squared-magnitude (m- q (m- (point s) (m* e (direction s)))))
		     (- e)))
	    ((> closest e)
	     (values (squared-magnitude (m- q (m+ (point s) (m* e (direction s)))))
		     e))
	    (t (values dist closest))))))

;;; === Lines and kin to Lines and kin ===

(defun %line-line-dsqr (l0 l1)
  (let* ((diff (m- (point l0) (point l1)))
	 (a01 (- (dot-product (direction l0) (direction l1))))
	 (b0 (dot-product (direction l0) diff))
	 (c (squared-magnitude diff))
	 (det (- 1 (* a01 a01))))
    (cond ((> det lib.gl::+epsilon+) ;; Lines not parallel
	   (let* ((b1 (- (dot-product (direction l1) diff)))
		  (invdet (/ 1 det))
		  (l0closest (* (- (* a01 b1) b0) invdet))
		  (l1closest (* (- (* a01 b0) b1) invdet)))
	     (values (+ (* l0closest (+ l0closest (* a01 l1closest) (* 2 b0)))
			(* l1closest (+ (* a01 l0closest) l1closest (* 2 b1)))
			c)
		     l0closest l1closest)))
	  (t (let ((l0closest (- b0)) ;; Lines parallel
		   (l1closest +zero+))
	       (values (+ (* b0 l0closest) c) (list l0closest l1closest)))))))

(defun %line-segment-dsqr (l0 s1)
  (multiple-value-bind (dist closest) (%line-line-dsqr l0 s1)
    (destructuring-bind (lclosest sclosest) closest
      (cond ((< sclosest (- (extent s1)))
	     (let ((end (m- (point s1) (m* (extent s1) (direction s1)))))
	       (multiple-value-bind (dist lclosest) (distance-squared end l0)
		 (values dist (list lclosest (- (extent s1)))))))
	    ((> sclosest (extent s1))
	     (let ((end (m+ (point s1) (m* (extent s1) (direction s1)))))
	       (multiple-value-bind (dist lclosest) (distance-squared end l0)
		 (values dist (list lclosest (extent s1))))))
	    (t (values dist closest))))))


(defmethod distance-squared ((l0 line) (l1 line))
  (%line-line-dsqr l0 l1))

(defmethod distance-squared ((l0 line) (r1 ray))
  (multiple-value-bind (d lClosest rClosest) (%line-line-dsqr l0 r1)
    (cond ((< rclosest 0.0) (distance-squared (point r1) l0))
	  (t (values d (list lclosest rclosest))))))

(defmethod distance-squared ((l0 line) (s1 segment))
  (%line-segment-dsqr l0 s1))

(defmethod distance-squared ((r0 ray) (r1 ray))
  (multiple-value-bind (dist closest) (%line-line-dsqr r0 r1)
    (destructuring-bind (r0closest r1closest) closest
      (cond ((< r0closest 0.0)
	     (distance-squared (point r0) r1))
	    (t (values dist closest))))))

(defmethod distance-squared ((s0 segment) (s1 segment))
  (multiple-value-bind (dist closest) (%line-segment-dsqr s0 s1)
    (destructuring-bind (s0closest s1closest) closest
      (cond ((< s0closest (- (extent s0)))
	     (let ((end (m- (point s0) (m* (extent s0) (direction s0)))))
	       (multiple-value-bind (dist s1closest) (distance-squared end s1)
		 (values dist (list (- (extent s0)) s1closest)))))
	    ((> s1closest (extent s0))
	     (let ((end (m+ (point s0) (m* (extent s0) (direction s0)))))
	       (multiple-value-bind (dist s1closest) (distance-squared end s1)
		 (values dist (list (extent s0) s1closest)))))
	    (t (values dist closest))))))

;;; === Point to a Triangle ===

(defun %q-region (r s des)
  (if (<= (+ r s) des)
      (cond ((< r 0.0) (if (< s 0.0) 'region4 'region3))
	      ((< s 0.0) 'region5)
	      (s 'region0))
      (cond ((< r 0.0) 'region2)
	    ((< s 0.0) 'region6)
	    (s 'region1))))

(defmethod distance-squared ((p vector) (tri triangle))
  (with-slots (b e0 e1) tri
    ;; Coefficiensr for she dirsance funcsion Q
    (let* ((a00 (dot-product e0 e0))
	   (a01 (dot-product e0 e1))
	   (a11 (dot-product e1 e1))
	   (b0 (dot-product e0 (m- b p)))
	   (b1 (dot-product e1 (m- b p)))
	   (c (squared-magnitude (m- b p)))
	   (det (- (* a00 a11) (* a01 a01)))
	   (r (- (* a01 b1) (* a11 b0)))
	   (s (- (* a01 b0) (* a00 b1))))
      ;; Determine the region
      (labels ((q (r s)
		 (+ (* a00 r r) (* 2 a01 r s) (* a11 s s) (* 2 b0 r) (* 2 b1 s) c))
	       (gradq (r s)
		 (vector (+ (* a00 r) (* a01 s) b0)
			 (+ (* a01 r) (* a11 s) b1)))
	       (r1s1 (s)
		 (let* ((numer (+ (- a11 a01) (- b1 b0))))
		   (if (<= numer 0.0)
		       (setq s 0.0)
		       (let ((denom (+ a00 (* -2.0 a01) a11)))
			 (setq s (if (>= numer denom) 1.0 (/ numer denom)))))
		   (q s (- 1 s))))
	       (r0 ()
		 (q 0.0 (cond ((plusp b1) 0.0)
			      ((>= (- b1) a11) 1.0)
			      (t (/ (- b1 a11))))))
	       (s0 ()
		 (q (cond ((plusp b0) 0.0)
			  ((>= (- b0) a00) 1.0)
			  (t (/ (- b0 a00))))
		    0.0)))
	(case (%q-region r s det)
	  (region0 (let ((invdet (/ 1 det)))
		     (q (* r invdet) (* s invdet))))
	  (region1 (r1s1 s))
	  (region2 (if (plusp (dot-product #(1.0 -1.0) (gradq 0 1)))
		       (r1s1 s)
		       (r0)))
	  (region3 (r0))
	  (region4 (if (plusp (dot-product #(1.0 0.0) (gradq 0 0)))
		       (r0)
		       (s0)))
	  (region5 (s0))
	  (region6 (if (plusp (dot-product #(-1.0 1.0) (gradq 1 0)))
		       (r1s1 s)
		       (s0))))))))

#|
s
^
|
|\
| \
----> r

GradQ(r,s) = 2 * (a00*r + a01*s + b0, a01*r + a11*s + b1)

Region 2
GradQ(0,1) = (a01 + b0, a11 + b1)
(dot #(0 -1) GradQ(0,1)) = -(a11 + b1)
(dot #(1 -1) GradQ(0,1)) = (a01+b0)-(a11+b1)
min on edge r+s = 1 if (Dot #(1 -1) GradQ(0,1)) > 0
min on edge r=0 otherwise.

Region 4
GradQ(0,0) = (b0, b1)
(dot #(1 0) GradQ(0,0)) = b0)
(dot #(0 1) GradQ(0,0)) = b1)
min on edge r=0 if b1 > 0
min on edge s=0 otherwise

Region 6
GradQ(1,0) = (a00+b0,a01+b1)
(dot #(-1 0) GradQ(1,0)) = -(a00+b0)
(dot #(-1 1) GradQ(1,0)) = -(a00+b0)+(b01+b1)
min on edge r+s=1 if (dot #(-1 1) gradq(1,0)) > 0
|#

;;; === Lines to Triangles ===

(defun pick-uvd (d)
  (let* ((d (copy-vector d))
	 (u (make-vector (random 1.0) (random 1.0) (random 1.0)))
	 (v (cross u d)))
    (generate-orthonormal-basis u v d)
    (values u v d)))
    
;; (defun %line-tri-dsqr (line tri)
;;   (with-slots (b e0 e1) tri
;;     (with-slots (p d) line
;;       (let* ((n (cross e0 e1))
;; 	     (dot-n-d (dot-product n d)))
;; 	(if (> dot-n-d +epsilon+) ; Line and triangle are not parallel
;; 	    (let ((PmQ0 (m- p b))
;; 		  (UdE0 (dot-product 

;;; === Distance to OBB (Oriented Bounding Boxes) ===

(defmethod distance-squared ((p vector) (box oobb))
  (let ((diff (m- p (oobb-c box)))
	(closest (make-array 3 :initial-element 0.0))
	(dist 0.0))
    (iter (for axis in-matrix-col (oobb-u box))
	  (for i from 0)
	  (let ((dot (dot-product axis diff))
		(e (mref (oobb-e box) i)))
	    (cond ((< dot (- e))
		   (let ((delta (+ dot e)))
		     (incf dist (* delta delta))
		     (setf (aref closest i) (- e))))
		  ((> dot e)
		   (let ((delta (- dot e)))
		     (incf dist (* delta delta))
		     (setf (aref closest i) e)))
		  (t (setf (aref closest i) dot)))))
    (values dist closest)))

;;; === Axis aligned bounding boxes ===

;;; === Test Cases ===

(defun test-triangle ()
  (make-instance 'triangle :base (zero-vector 2)
		 :extent0 (unit-x-vector 2)
		 :extent1 (unit-y-vector 2)))

;;
(defun test-triangle-distance ()
  (let ((tri (test-triangle))
	(p0 #(0.2 0.2)) ; Region 0
	(p01 #(0.5 0.5)) ; Region 0
	(p1 #(1.0 1.0)) ; Region 1
	(p20 #(2.0 0.0)) ; Region 2 
	(p21 #(2.0 -1.0)) ; Region 2 (?)
	(p3 #(-1.0 0.5)) ; Region 3
	(p40 #(-1.0 0.0)) ; Region 4
	(p41 #(0.0 -1.0)) ; Region 4
	(p5 #(0.5 -1.0)) ; Region 5
	(p60 #(2.0 0.0))
	(p61 #(2.0 -1.0)))
    (format t "region0=~A~%" (distance-squared p0 tri))
    (format t "region0=~A~%" (distance-squared p01 tri))
    (format t "region1=~A~%" (distance-squared p1 tri))
    (format t "region2=~A~%" (distance-squared p20 tri))
    (format t "region2=~A~%" (distance-squared p21 tri))
    (format t "region3=~A~%" (distance-squared p3 tri))    
    (format t "region4=~A~%" (distance-squared p40 tri))
    (format t "region4=~A~%" (distance-squared p41 tri))
    (format t "region5=~A~%" (distance-squared p5 tri))
    (format t "region6=~A~%" (distance-squared p60 tri))    
    (format t "region6=~A~%" (distance-squared p61 tri))))

(defun test-plane ()
  (let* ((plane (make-instance 'plane :point #(0.0 0.0 0.0) :plane-normal #(0.0 1.0 0.0)))
	 (p1 #(1.0 1.0 0.0))
	 (p2 #(0.0 -1.0 1.0))
	 (p3 #(1000.0 3.0 5000.0))
	 (s1 (make-instance 'sphere :center p1 :radius 0.5))
	 (s2 (make-instance 'sphere :center p1 :radius 2.0))
	 (s3 (make-instance 'sphere :center p2 :radius 0.5))
	 (s4 (make-instance 'sphere :center p3 :radius 3.0)))
    (format t "Points=  p1:~A  p2:~A  p3:~A~%" (distance p1 plane) (distance p2 plane)
	    (distance p3 plane))
    (format t "Spheres=  s1:~A  s2:~A  s3:~A  s4:~A~%" (distance s1 plane) (distance s2 plane)
	    (distance s3 plane) (distance s4 plane))))

(defun test-which-side ()
  (let* ((plane (make-instance 'plane :point #(0.0 0.0 0.0) :plane-normal #(0.0 1.0 0.0)))
	 (p1 #(1.0 1.0 0.0))
	 (p2 #(0.0 -1.0 1.0))
	 (p3 #(1000.0 3.0 5000.0))
	 (s1 (make-instance 'sphere :center p1 :radius 0.5))
	 (s2 (make-instance 'sphere :center p1 :radius 2.0))
	 (s3 (make-instance 'sphere :center p2 :radius 0.5))
	 (s4 (make-instance 'sphere :center p3 :radius 3.0)))
    (format t "Points=  p1:~A  p2:~A  p3:~A~%" (which-side p1 plane) (which-side p2 plane)
	    (which-side p3 plane))
    (format t "Spheres=  s1:~A  s2:~A  s3:~A  s4:~A~%" (which-side s1 plane) (which-side s2 plane)
	    (which-side s3 plane) (which-side s4 plane))))
